// 控制算法: 梯形加减速控制(加速-匀速-减速)
// 调用频率: 每秒5000次(5kHz)
// 输入参数:
// 角加速度 (单位: 编码器计数/周期²)
// 最大角速度 RPM (内部使用的单位是 编码器计数/周期)
// 起始位置 (编码器角度，一圈16384，范围-2³¹~2³¹-1)
// 终止位置 (同上)
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "hardware/sync.h"
#include "trapezoid.h"

Trapezoid_FIFO trap_fifo_ctrl;

// 初始化函数（需在程序启动时调用）
void trapezoid_fifo_init(Trapezoid_FIFO* fifo) 
{
    // 申请两个硬件锁
    spin_lock_t alloc_lock_instance;
    spin_lock_t queue_lock_instance;
    alloc_lock_instance =  spin_lock_claim_unused(true);
    queue_lock_instance = spin_lock_claim_unused(true);
    
    fifo->alloc_lock = spin_lock_init(alloc_lock_instance);
    fifo->queue_lock = spin_lock_init(queue_lock_instance);
    
    fifo->allocated = 0;
    fifo->queue_head = 0;
    fifo->queue_tail = 0;
    fifo->queue_count = 0;
}

// 分配Trapezoid（核心0）
Trapezoid *trapezoid_alloc(Trapezoid_FIFO *fifo) 
{
    if (!fifo) return NULL;
    
    uint32_t save = spin_lock_blocking(fifo->alloc_lock);
    Trapezoid *t = NULL;
    for (int i = 0; i < FIFO_LENGTH; ++i) {
        if (!(fifo->allocated & (1u << i))) {
            fifo->allocated |= (1u << i);
            t = &fifo->trap_fifo[i];
            break;
        }
    }
    spin_unlock(fifo->alloc_lock, save);
    return t;
}

// 释放Trapezoid（核心1）
void trapezoid_free(Trapezoid_FIFO *fifo, Trapezoid *t) 
{
    if (!fifo || !t) return;
    int index = t - fifo->trap_fifo;
    if (index < 0 || index >= FIFO_LENGTH) return;
    
    uint32_t save = spin_lock_blocking(fifo->alloc_lock);
    fifo->allocated &= ~(1u << index);
    spin_unlock(fifo->alloc_lock, save);
}

// 入队（核心0）
int trapezoid_push(Trapezoid_FIFO *fifo, Trapezoid *t) 
{
    if (!fifo || !t) return -1;
    int index = t - fifo->trap_fifo;
    if (index < 0 || index >= FIFO_LENGTH) return -1;

    // 快速检查是否已分配（非原子操作，后续还有二次检查）
    if (!(fifo->allocated & (1u << index))) return -2;

    uint32_t save = spin_lock_blocking(fifo->queue_lock);
    if (fifo->queue_count >= FIFO_LENGTH) {
        spin_unlock(fifo->queue_lock, save);
        return -3;
    }
    
    // 二次原子检查分配状态
    uint32_t alloc_save = spin_lock_blocking(fifo->alloc_lock);
    bool is_allocated = (fifo->allocated & (1u << index));
    spin_unlock(fifo->alloc_lock, alloc_save);
    
    if (!is_allocated) {
        spin_unlock(fifo->queue_lock, save);
        return -2;
    }
    
    fifo->queue[fifo->queue_tail] = t;
    fifo->queue_tail = (fifo->queue_tail + 1) % FIFO_LENGTH;
    fifo->queue_count++;
    spin_unlock(fifo->queue_lock, save);
    return 0;
}

// 出队（核心1）
Trapezoid *trapezoid_pop(Trapezoid_FIFO *fifo) 
{
    if (!fifo) return NULL;
    
    uint32_t save = spin_lock_blocking(fifo->queue_lock);
    if (fifo->queue_count == 0) {
        spin_unlock(fifo->queue_lock, save);
        return NULL;
    }
    
    Trapezoid *t = fifo->queue[fifo->queue_head];
    fifo->queue_head = (fifo->queue_head + 1) % FIFO_LENGTH;
    fifo->queue_count--;
    spin_unlock(fifo->queue_lock, save);
    return t;
}


int trapezoid_calculate_motion(Trapezoid* trap, float x0, float x1, float v0, float v1, float vmax, float a) 
{
    
    const float to_rpm = 16384.0 / 5000.0 / 60.0;
    v0 = v0 * to_rpm;
    v1 = v1 * to_rpm;
    vmax = vmax * to_rpm;
    // 参数有效性检查
    float vmax_used = fabsf(vmax);
    float a_used = fabsf(a);
    if (vmax_used < 1e-6f) {
        printf("Error: 过小的最大速度\n");
        return 1;
    }
    if (a_used < 1e-6f) {
        printf("Error: 过小的加速度\n");
        return 2;
    }
    if (fabsf(v0) > vmax_used || fabsf(v1) > vmax_used) {
        printf("Error: 初始速度或目标速度超过最大允许速度\n");
        return 3;
    }

    // 计算位移和方向
    float S = x1 - x0;
    int direction = (S >= 0) ? 1 : -1;
    float signed_vmax = vmax_used * direction;
    float a_accel = a_used * direction;
    float a_decel = -a_used * direction;

    // 计算理论最大加速/减速时间
    float Ta_max = (signed_vmax - v0) / a_accel;
    float Td_max = (v1 - signed_vmax) / a_decel;

    // 计算理论最大位移
    float Sa_max = (signed_vmax*signed_vmax - v0*v0) / (2 * a_accel);
    float Sd_max = (v1*v1 - signed_vmax*signed_vmax) / (2 * a_decel);
    float S_needed = Sa_max + Sd_max;

    // 判断运动情景
    float Tv = (S - S_needed) / signed_vmax;
    if (Tv >= 0) {
        // 情景1: 能加速到最高速度
        trap->Ta = Ta_max;
        trap->Td = Td_max;
        trap->Tv = Tv;
        trap->v_c = signed_vmax;
        trap->T = trap->Ta + trap->Tv + trap->Td;
    } else {
        // 情景2: 无法达到最高速度
        float abs_S = fabsf(S);
        float v_c_sq = (2 * a_used * abs_S + v0*v0 + v1*v1) / 2.0f;
        if (v_c_sq < 0) {// 数学上v_c_sq一定是≥0的，这段代码用于防止浮点误差的影响
            printf("Error: 无法计算中间速度\n");
            return 4;
        }
        float v_c_abs = sqrtf(v_c_sq);
        float v_c = v_c_abs * direction;

        trap->Ta = (v_c - v0) / a_accel;
        trap->Td = (v1 - v_c) / a_decel;
        trap->Tv = 0;
        trap->v_c = v_c;
        trap->T = trap->Ta + trap->Td;
    }

    // 检查时间参数
    if (trap->Ta < 0 || trap->Td < 0) {
        printf("Error: 计算得到负时间\n");
        return 4;
    }

    // 计算关键点位置
    trap->x0 = x0;
    trap->x1 = x1;
    trap->v0 = v0;
    trap->v1 = v1;
    trap->a_accel = a_accel;
    trap->a_decel = a_decel;
    trap->x_acc_end = x0 + v0 * trap->Ta + 0.5f * a_accel * trap->Ta * trap->Ta;
    trap->x_vel_end = trap->x_acc_end + trap->v_c * trap->Tv;

    return 0;
}

float trapezoid_get_pos(Trapezoid* trap, float t) {
    if (t < 0) return trap->x0;
    if (t > trap->T) return trap->x1;

    if (t <= trap->Ta) {
        float dt = t;
        return trap->x0 + trap->v0 * dt + 0.5f * trap->a_accel * dt * dt;
    } else if (t <= trap->Ta + trap->Tv) {
        float dt = t - trap->Ta;
        return trap->x_acc_end + trap->v_c * dt;
    } else {
        float dt = t - trap->Ta - trap->Tv;
        return trap->x_vel_end + trap->v_c * dt + 0.5f * trap->a_decel * dt * dt;
    }
}

float trapezoid_get_velocity(Trapezoid* trap, float t) {
    if (t < 0) return trap->v0;
    if (t > trap->T) return trap->v1;

    if (t <= trap->Ta) {
        return trap->v0 + trap->a_accel * t;
    } else if (t <= trap->Ta + trap->Tv) {
        return trap->v_c;
    } else {
        float dt = t - trap->Ta - trap->Tv;
        return trap->v_c + trap->a_decel * dt;
    }
}

// 测试用例
// int main() {
//     Trapezoid trap;
//     int ret = trapezoid_calculate_motion(&trap, 0, 16384, 0, 0, 109, 0.5);
//     if (ret != 0) return ret;

//     printf("位置轨迹: [");
//     for (float t = 0.0f; t < trap.T; t += 1.0f) {
//         printf("%.1f, ", trapezoid_get_pos(&trap, t));
//     }
//     printf("]\n");

//     return 0;
// }